#include "DaneWej.h"
#include "Skan3D.h"
#include "Punkt.h"

#include <math.h>
#include <sstream>
#include <vector>
using namespace std;

/**
 *
 *
 */
Skan3D* DaneWej::wczytajPlikLaser(const char plik[], int rozdzielczosc) {
    string linia;
    ifstream wczytywanie(plik); //ifstream - tylko odczyt
    int thePionAng = 0; //odchylenie w pionie
    int _scan2DSize = rozdzielczosc; //rozmiar tabicy skanu 2D, czyli ilosc pomiarow na zakres 181 lub 361
    int zakresSkanu = 180; //to w jakim faktycznie zakresie pracuje skaner prawie zawsze 180

    vector<Punkt*> wszVec; //do przechowywania skanów2D

    //1. Odczyt linia po linii
    bool wasLaserF = false; //flaga, że była funkcja Laser i pomijam inne lasery, aż będzie PTZ
    int _laserCnt = 0; //licznik pomocniczy (tylko do testów)
    while (getline(wczytywanie, linia)) {

        //2. Tworzę tablicę 2D do skanu 2D

        //Pomijam linie, które nie mają żądanych komend
        if (linia.find("laser") == string::npos && linia.find("ptz") == string::npos) continue;
        //cout << "Badam linie: --|" << linia << "|--" <<endl; //wypisanie linia po linii

        //2. Zapisanie wyrazow w linii w Vectorze.
        stringstream inss(linia);
        vector<string> vec;

        do {
            string str; //string pomocniczy
            inss >> str;
            if (str.size() > 0)
                vec.push_back(str);
        } while (!inss.eof());

        //4. Obrabiam w zależności, czy to jest laser, czy ptz
        if (vec[3] == "ptz") {
            //if (wasLaserF) wasLaserF = false; //kasuję flagę.
            istringstream iss(vec[7]);
            int _angle = thePionAng; //zapamiętuję stary kąt
            iss >> thePionAng;
            if (_angle != thePionAng) //cz. kąt w pionie sie zmienił i mogę czytać laser
                wasLaserF = false;

            //// cout<< "thePionAng: " << thePionAng << endl;

        } else if (vec[3] == "laser" && !wasLaserF) {
            wasLaserF = true; //ustawiam flagę, żeby ignorować kolejne komendy Laser

            //---
            _laserCnt++;
            ////cout << "Laser: " << _laserCnt << endl;
            //===

            //Zapisuje pkt do tablicy[]
            Punkt* pktT = new Punkt[_scan2DSize]; //skan2D
            //............. pktT jak zmienia wartość, to automatycznie zmienia się wartość zapisana w wszVec
            for (int i = 0; i < _scan2DSize; i++) {
                //Zamieniam String na int
                istringstream iss(vec[10 + i * 2]);
                double r;
                iss >> r;
                Punkt p(r,  thePionAng, i * zakresSkanu / (_scan2DSize - 1), RTetaFi);
                pktT[i] = p;
                //if (i<10) cout << "r: " << r << ", pktT[" << i << "]: " << pktT[i].r <<endl;
            } //koniec for

            //Zapisuję cały skan2D w vectorze
            /*---
            cout << " ---" << endl;
            for (int i=0; i<wszVec.size(); i++)
                cout << "b)wszVec[" << i << "][0]: " << wszVec[i][0].r  << ", [1]: " << wszVec[i][1].r << endl;
            cout << "a) wszVec.s: " << wszVec.size() << endl;
            for (int i=0; i<6; i++) cout << "pktT[" << i <<"]:" << pktT[i].r;
            cout << endl;
             */
            wszVec.push_back(pktT);

            /*/---
            cout << " -" << endl;
            for (int i=0; i<wszVec.size(); i++)
                cout << "b)wszVec[" << i << "][0]: " << wszVec[i][0].r  << ", [1]: " << wszVec[i][1].r << endl;
            cout << "===" << endl;
             */
        } //koniec if-else
    } //koniec while

    wczytywanie.close(); //zamykam strumien do pliku

    //Wysylam otrzymane wyniki do Skanu 3D
    //Zamieniam wektor na tabl. dwuwymiarową (na piechotę, bo inaczej nie potrafię)
    //Tworzę tablicę dwuwymairową
    Punkt ** pktT2 = new Punkt * [wszVec.size()];
    for (unsigned int i = 0; i < wszVec.size(); i++)
        pktT2[i] = new Punkt[_scan2DSize];

    //Wypełniam tablice dwuwymiarową
    for (unsigned int i = 0; i < wszVec.size(); i++) {
        for (int j = 0; j < _scan2DSize; j++) {
            //if (i<6 && j<6) cout << "wszVec[" << i << "][" << j << "]: " << wszVec[i][j].r << endl;
            pktT2[i][j] = wszVec[i][j];
        }
    }
    /*///
        for (int i=0; i<2; i++)
            for (int j=0; j<10; j++)
                cout << "pktT2[" << i << "][" << j << "]: " << pktT2[i][j].r << endl;
     */
    return new Skan3D(pktT2, wszVec.size(), _scan2DSize);

}
//==============================================================================

/**
 *
 *
 */
Skan3D* DaneWej::wczytajPlikLaser(string plik, int rozdzielczosc) {
    cout << "wczytajPlikLaser: " << plik << endl;
    // return DaneWej::wczytajPlikLaser(plik.c_str());
    return new Skan3D(NULL, 0, 0);
}
//==============================================================================

/**
 *
 *
 */
Skan3D* DaneWej::wczytajPlikXYZ(const char plik[], int rozdzielczosc) {
    Skan3D* skan = new Skan3D(NULL, 0, 0);


    return skan;
}
//==============================================================================

/**
 *
 *
 */
Skan3D* DaneWej::wczytajPlikXYZ(string plik, int rozdzielczosc) {
    return DaneWej::wczytajPlikXYZ(plik.c_str(), rozdzielczosc);
}
//==============================================================================



Skan3D* DaneWej::wczytajPlikLaser2(const char plik[], int rozdzielczosc) {
    string linia;
    ifstream wczytywanie(plik); //ifstream - tylko odczyt
    double thePionAng = 0; //odchylenie w pionie
    int _scan2DSize = rozdzielczosc; //rozmiar tabicy skanu 2D, czyli ilosc pomiarow na zakres 181 lub 361
    int zakresSkanu = 180; //to w jakim faktycznie zakresie pracuje skaner prawie zawsze 180

    vector<Punkt*> wszVec; //do przechowywania skanów2D

    //1. Odczyt linia po linii
    bool wasLaserF = false; //flaga, że była funkcja Laser i pomijam inne lasery, aż będzie PTZ
    int _laserCnt = 0; //licznik pomocniczy (tylko do testów)
    while (getline(wczytywanie, linia)) {

        //2. Tworzę tablicę 2D do skanu 2D

        //Pomijam linie, które nie mają żądanych komend
        if (linia.find("laser") == string::npos && linia.find("ptz") == string::npos) continue;
        //cout << "Badam linie: --|" << linia << "|--" <<endl; //wypisanie linia po linii

        //2. Zapisanie wyrazow w linii w Vectorze.
        stringstream inss(linia);
        vector<string> vec;

        do {
            string str; //string pomocniczy
            inss >> str;
            if (str.size() > 0)
                vec.push_back(str);
        } while (!inss.eof());

        //4. Obrabiam w zależności, czy to jest laser, czy ptz
        if (vec[3] == "ptz") {
            //if (wasLaserF) wasLaserF = false; //kasuję flagę.
            istringstream iss(vec[7]);
            double _angle = thePionAng; //zapamiętuję stary kąt
            iss >> thePionAng;
            if (Abs(_angle - thePionAng) > 0.01) //cz. kąt w pionie sie zmienił i mogę czytać laser
                wasLaserF = false;

            //// cout<< "thePionAng: " << thePionAng << endl;

        } else if (vec[3] == "laser" && !wasLaserF) {
            wasLaserF = true; //ustawiam flagę, żeby ignorować kolejne komendy Laser
            _laserCnt++;//==============
            //Zapisuje pkt do tablicy[]
            Punkt* pktT = new Punkt[_scan2DSize]; //skan2D
            //............. pktT jak zmienia wartość, to automatycznie zmienia się wartość zapisana w wszVec
            for (int i = 0; i < _scan2DSize; i++) {
                //Zamieniam String na int
                istringstream iss(vec[10 + i * 2]);
                double r;
                iss >> r;
                Punkt p(r, i * zakresSkanu / (_scan2DSize - 1), thePionAng, RTetaFi);
                pktT[i] = p;
                //if (i<10) cout << "r: " << r << ", pktT[" << i << "]: " << pktT[i].r <<endl;
            } //koniec for


            wszVec.push_back(pktT);
        } //koniec if-else
    } //koniec while

    wczytywanie.close(); //zamykam strumien do pliku

    //Wysylam otrzymane wyniki do Skanu 3D
    //Zamieniam wektor na tabl. dwuwymiarową (na piechotę, bo inaczej nie potrafię)
    //Tworzę tablicę dwuwymairową
    Punkt ** pktT2 = new Punkt * [wszVec.size()];
    for (unsigned int i = 0; i < wszVec.size(); i++)
        pktT2[i] = new Punkt[_scan2DSize];

    //Wypełniam tablice dwuwymiarową
    for (unsigned int i = 0; i < wszVec.size(); i++) {
        for (int j = 0; j < _scan2DSize; j++) {
            //if (i<6 && j<6) cout << "wszVec[" << i << "][" << j << "]: " << wszVec[i][j].r << endl;
            pktT2[i][j] = wszVec[i][j];
        }
    }
   return new Skan3D(pktT2, wszVec.size(), _scan2DSize);

}

double Abs(double x){
    if (x < 0) return -x;
    return x;
}
